#include "StepperMotor.h"
#include <cmath>
static HC595_Data_t hc595_data = {1UL};// 全局数据位状态存储（使用74HC595位域结构体）

// 构造函数：初始化步进电机（基于MotorID）
StepperMotor::StepperMotor(MotorID motorId, int8_t direction, float stepsPerMm, float maxPosition)
    :  _motorId(motorId)          // 电机ID（关联数据位）
      ,_direction(direction)      // 电机方向（1或-1）
      ,_stepsPerMm(stepsPerMm)    // 每毫米步数
      ,_currentPosition(0)        // 当前位置初始化为0
      ,_targetPosition(0)         // 目标位置初始化为0
      ,_maxPosition(maxPosition)  // 默认最大位置200mm =====新加，轴最大行程
      ,_isMoving(false)           // 初始状态为静止
      ,_stepState(false)          // 步进信号初始状态为低电平
      ,_isEnabled(false)          // 初始状态为禁用
      ,_currentFeedrate(0)        // 当前速度初始化为0
      ,_targetFeedrate(0)         // 目标速度初始化为0
      ,_currentAcceleration(0)    // 当前加速度初始化为0
      ,_targetAcceleration(0)     // 目标加速度初始化为0
      ,_startFeedrate(0)          // 起始速度初始化为0
      ,_maxFeedrate(0)            // 最大速度初始化为0
      ,_lastStepTime(0)           // 上次步进时间初始化为0
      ,_stepInterval(1000)        // 步进间隔初始化为1ms
      ,_accelStartTime(0)         // 加速开始时间初始化为0
      , _needStep(false)
{
}

// 初始化电机（通过motor_set_signal设置默认状态）
void StepperMotor::init()
{
  _currentPosition = 0;
  _targetPosition = 0;
  _isEnabled = false;

  // 初始化EN（禁用）、STEP（低）、DIR（低）信号
  motor_set_signal(_motorId, MOTOR_EN, 1);   // 高电平禁用
  motor_set_signal(_motorId, MOTOR_STEP, 0); // 步进初始低电平
  motor_set_signal(_motorId, MOTOR_DIR, 0);  // 方向初始低电平
  motor_update_outputs();                    // 同步到74HC595
}

// 启用电机（设置EN信号为低电平使能）
void StepperMotor::enable()
{
  printf("Enabling motor\r\n");
  if (_isEnabled)
    return;

  motor_set_signal(_motorId, MOTOR_EN, 0); // EN=0（使能）
  motor_update_outputs();                  // 同步输出
  _isEnabled = true;
}

// 禁用电机（设置EN信号为高电平禁用）
void StepperMotor::disable()
{
  if (!_isEnabled)
    return;

  motor_set_signal(_motorId, MOTOR_EN, 1); // EN=1（禁用）
  motor_update_outputs();                  // 同步输出
  _isEnabled = false;
}

// 设置电机运动方向（通过DIR信号）
void StepperMotor::setDirection(bool forward)
{
  bool actualDirection = forward;
  if (_direction < 0)
    actualDirection = !actualDirection; // 方向反转

  motor_set_signal(_motorId, MOTOR_DIR, actualDirection); // DIR=实际方向（1/0）
  motor_update_outputs();                                 // 同步输出
}

// 执行单步运动（生成STEP脉冲）==== 改为在MotionController::update()中统一脉冲
void StepperMotor::step()
{
  // if (!_isEnabled)
  //   return;

  // _stepState = !_stepState;                                                           // 翻转步进信号状态
  // motor_set_signal(_motorId, MOTOR_STEP, _stepState ? GPIO_PIN_SET : GPIO_PIN_RESET); // 输出步进信号
  // motor_update_outputs();
  // delay_us(1); // 延时1us
}

// 设置当前位置
void StepperMotor::setCurrentPosition(float position)
{
  _currentPosition = position;
}

// 获取当前位置
float StepperMotor::getCurrentPosition() const
{
  return _currentPosition;
}

// 获取目标位置
float StepperMotor::getTargetPosition() const
{
  return _targetPosition;
}

// 设置目标位置（带限位）
void StepperMotor::setTargetPosition(float position)
{
  printf("Setting target position: %f\r\n", position);

  // 限制在最大位置范围内 ====新加
  if (position > _maxPosition)
    position = _maxPosition;
  else if (position < -_maxPosition)
    position = -_maxPosition;

  _targetPosition = position; // 设置目标位置
  _isMoving = true;           // 设置运动状态
  _accelStartTime = micros(); // 记录加速开始时间

  // 设置方向并启用电机，设置运动方向，考虑_direction参数
  bool forward = _targetPosition > _currentPosition;
  setDirection(forward);

  _startFeedrate = _currentFeedrate; // 设置起始速度
  _maxFeedrate = _targetFeedrate;    // 设置最大速度

  calculateStepInterval(); // 计算步进间隔
  enable();

  printf("Motor enabled, moving to position: %f\r\n", position);
}

// 检查是否正在运动
bool StepperMotor::isMoving() const
{
  return _isMoving;
}

// 设置进给速度
void StepperMotor::setFeedrate(float feedrate)
{
  _targetFeedrate = feedrate;
  _currentFeedrate = feedrate; // 立即生效（简化模型）
  calculateStepInterval();
}

// 设置加速度
void StepperMotor::setAcceleration(float acceleration)
{
  if (acceleration < 0)
    acceleration = 0;
  _targetAcceleration = acceleration; // 设置目标加速度
}

// 获取当前速度
float StepperMotor::getCurrentFeedrate() const
{
  return _currentFeedrate;
}

// 获取当前加速度
float StepperMotor::getCurrentAcceleration() const
{
  return _currentAcceleration;
}

// 计算步进间隔（根据当前速度）
void StepperMotor::calculateStepInterval()
{
  if (_currentFeedrate <= 0)
  {
    _stepInterval = 1000;
    return;
  }
  float stepsPerSecond = _currentFeedrate * _stepsPerMm; // 步数/s

  _stepInterval = static_cast<uint32_t>(1000000.0f / stepsPerSecond); // 微秒/步
  if (_stepInterval < 100)
    _stepInterval = 100; // ?? 最小385us,即脉冲周期≥385μs,1/2600≈385μs,再大可能电机会丢步
  // printf("==== _currentFeedrate: %f, _stepInterval: %lu\r\n", _currentFeedrate, _stepInterval);
}

// // 更新运动速度（加速/减速控制）
// void StepperMotor::updateSpeed()
// {
//   if (!_isMoving)
//     return;

//   unsigned long currentTime = micros();                             // 获取当前时间
//   float elapsedTime = (currentTime - _accelStartTime) / 1000000.0f; // 已过时间，转换为秒

//   // 加速阶段
//   if (_currentFeedrate < _maxFeedrate && _targetAcceleration > 0)
//   {
//     _currentFeedrate = _startFeedrate + (_targetAcceleration * elapsedTime);
//     if (_currentFeedrate > _maxFeedrate)
//       _currentFeedrate = _maxFeedrate;
//   }

//   // 减速阶段
//   float distanceToTarget = (float)my_abs(_targetPosition - _currentPosition);
//   if (distanceToTarget > 0 && _targetAcceleration > 0)
//   {
//     float decelDistance = (_currentFeedrate * _currentFeedrate) / (2 * _targetAcceleration);
//     if (distanceToTarget <= decelDistance)
//     {
//       // _currentFeedrate = my_sqrt(2 * _targetAcceleration * distanceToTarget); // ====有点不准
//       _currentFeedrate = sqrtf(2 * _targetAcceleration * distanceToTarget);
//     }
//   }

//   calculateStepInterval();
// }

void StepperMotor::updateSpeed() // ==== 调试用
{
  if (!_isMoving)
    return;

  // ==== 删除原有复杂加速逻辑 ====
  // 简化为直接使用目标速度（实际项目应保留平滑加速）
  _currentFeedrate = _targetFeedrate;
  calculateStepInterval();
}

// 计算速度
float StepperMotor::calculateSpeed(float distance)
{
  return _targetFeedrate; // 直接使用目标速度
}

// // 更新电机状态
// void StepperMotor::update()
// {
//   if (!_isMoving || !_isEnabled)
//     return;

//   updateSpeed(); // 更新当前速度

//   unsigned long currentTime = micros();
//   if (currentTime - _lastStepTime >= _stepInterval)
//   {
//     _lastStepTime = currentTime;
//     step(); // 执行一步

//     // 更新当前位置
//     float stepDistance = 1.0f / _stepsPerMm;
//     if (_targetPosition > _currentPosition)
//     {
//       _currentPosition += stepDistance;
//       if (_currentPosition >= _targetPosition)
//         _currentPosition = _targetPosition;
//     }
//     else
//     {
//       _currentPosition -= stepDistance;
//       if (_currentPosition <= _targetPosition)
//         _currentPosition = _targetPosition;
//     }

//     // 检查是否到达目标位置
//     const float tolerance = 0.001f; // 根据精度需求调整
//     if (fabs(_currentPosition - _targetPosition) <= tolerance)
//     {
//       _isMoving = false;
//       _currentFeedrate = 0;
//       _stepState = false; // 确保步进信号回到低电平
//       motor_set_signal(_motorId, MOTOR_STEP, 0);
//     }
//   }
// }

// =========== new =============
// 仅判断是否需要步进，不执行实际脉冲（无延时）
void StepperMotor::update()
{
  if (!_isMoving || !_isEnabled)
  {
    _needStep = false;
    return;
  }
  updateSpeed();
  // printf("Interval: %lu\r\n", _stepInterval);
  unsigned long currentTime = micros();
  if (currentTime - _lastStepTime >= _stepInterval)
  {
    _needStep = true;
    _lastStepTime = currentTime;
  }
  else
  {
    _needStep = false;
  }
}

// 仅生成步进信号（无延时）
void StepperMotor::generateStepSignal()
{
  if (!_needStep)
    return;
  _stepState = !_stepState;
  motor_set_signal(_motorId, MOTOR_STEP, _stepState ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

// 仅更新位置（无IO操作和延时）
void StepperMotor::updatePosition()
{
  if (!_needStep)
    return;
  float stepDistance = 1.0f / _stepsPerMm; // 毫米每步
  if (_targetPosition > _currentPosition)
  {
    _currentPosition += stepDistance;
    if (_currentPosition >= _targetPosition)
      _currentPosition = _targetPosition;
  }
  else
  {
    _currentPosition -= stepDistance;
    if (_currentPosition <= _targetPosition)
      _currentPosition = _targetPosition;
  }
  // 检查是否到达目标位置
  const float tolerance = 0.001f;
  if (fabs(_currentPosition - _targetPosition) <= tolerance)
  {
    _isMoving = false;
    _currentFeedrate = 0;
    _stepState = false;
    motor_set_signal(_motorId, MOTOR_STEP, 0);
  }
  _needStep = false;
}

// 停止电机运动
void StepperMotor::stop()
{
  _isMoving = false;    // 停止运动
  _currentFeedrate = 0; // 速度清零
  _stepState = false;   // 确保步进信号回到低电平
  motor_set_signal(_motorId, MOTOR_STEP, 0);
  disable();
}

// 设置电机信号（更新数据位状态）
void motor_set_signal(MotorID motor, MotorSignalType signal, uint8_t state)
{
  // 根据MotorID和SignalType映射到对应的M1-M10的信号位
  switch (motor)
  {
  case MOTOR_A: // MOTOR_A → M1
    if (signal == MOTOR_EN)
      hc595_data.outputs.M1_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M1_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M1_DIR = state;
    break;

  case MOTOR_B: // MOTOR_B → M2
    if (signal == MOTOR_EN)
      hc595_data.outputs.M2_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M2_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M2_DIR = state;
    break;

  case MOTOR_C: // MOTOR_C → M3
    if (signal == MOTOR_EN)
      hc595_data.outputs.M3_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M3_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M3_DIR = state;
    break;

  case MOTOR_D: // MOTOR_D → M4
    if (signal == MOTOR_EN)
      hc595_data.outputs.M4_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M4_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M4_DIR = state;
    break;

  case MOTOR_E: // MOTOR_E → M5
    if (signal == MOTOR_EN)
      hc595_data.outputs.M5_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M5_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M5_DIR = state;
    break;

  case MOTOR_L: // MOTOR_L → M6
    if (signal == MOTOR_EN)
      hc595_data.outputs.M6_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M6_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M6_DIR = state;
    break;

  case MOTOR_M: // MOTOR_M → M7
    if (signal == MOTOR_EN)
      hc595_data.outputs.M7_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M7_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M7_DIR = state;
    break;

  case MOTOR_N: // MOTOR_N → M8
    if (signal == MOTOR_EN)
      hc595_data.outputs.M8_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M8_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M8_DIR = state;
    break;

  case MOTOR_O: // MOTOR_O → M9
    if (signal == MOTOR_EN)
      hc595_data.outputs.M9_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M9_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M9_DIR = state;
    break;

  case MOTOR_P: // MOTOR_P → M10
    if (signal == MOTOR_EN)
      hc595_data.outputs.M10_EN = state;
    if (signal == MOTOR_STEP)
      hc595_data.outputs.M10_STEP = state;
    if (signal == MOTOR_DIR)
      hc595_data.outputs.M10_DIR = state;
    break;

  default:
    break;
  }
}

// 更新输出（将数据位状态同步到74hc595）
void motor_update_outputs(void)
{
  // printf("hc595_data.raw:   %lu\r\n",hc595_data.raw);
  hc595_send(hc595_data.raw); // 发送位域结构体原始值到74HC595
}

// ------------------------------
// 时间及工具函数
// ------------------------------
TIM_HandleTypeDef htimx; // 使用定时器2实现us延时

void micros_init(void)
{
  __HAL_RCC_TIM2_CLK_ENABLE();
  htimx.Instance = TIM2;
  htimx.Init.Prescaler = SystemCoreClock / 1000000 - 1; //  数值大小随时钟数改变，使计数器为1us计数   // 除的数是频率 1000000 = 1M
  // printf("htimx.Init.Prescaler:  %lu  \r\n",htimx.Init.Prescaler);
  htimx.Init.CounterMode = TIM_COUNTERMODE_UP;
  htimx.Init.Period = 0xFFFFFFFF;
  htimx.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; // 新加，禁用自动重载预装载寄存器
  HAL_TIM_Base_Init(&htimx);
  HAL_TIM_Base_Start(&htimx);
}

// uint32_t micros(void) // 获取当前时间（微秒）
// {
//   return (uint32_t)__HAL_TIM_GET_COUNTER(&htimx);
// }

// 移除micros_init()和TIM2相关代码，修改micros()为：
uint32_t micros(void) // 和main.cpp中的delay_us统一
{
  return DWT->CYCCNT / (SystemCoreClock / 1000000);
}

// 绝对值实现
int my_abs(int x)
{
  return x < 0 ? -x : x;
}

// 平方根实现（牛顿迭代法）
float my_sqrt(float x)
{
  if (x <= 0)
    return 0; // 处理非法输入
  float guess = x * 0.5f;
  for (int i = 0; i < 4; i++)
    guess = 0.5f * (guess + x / guess);
  return guess;
}